#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    shoulderPot,    sensorPotentiometer)
#pragma config(Sensor, in2,    elbowPot,       sensorPotentiometer)
#pragma config(Sensor, dgtl5,  autoTrigger,    sensorTouch)
#pragma config(Sensor, dgtl6,  sonar,          sensorSONAR_inch)
#pragma config(Sensor, I2C_1,  leftEncoder,    sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  rightEncoder,   sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           frontLeft,     tmotorVex269_HBridge, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port2,           frontRight,    tmotorVex269_MC29, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port3,           backLeft,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           backRight,     tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           shoulderMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           shoulderMotor2, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           elbowMotor,    tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "functions.c"
/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

int BOT_ID = 1;

void pre_auton()
{
	bStopTasksBetweenModes = true;
	string WHEEL_TYPE ="omni";
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	writeDebugStream("Autonomous Started...");
	driveStraight(2, BOT_ID);
}
/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	// User control code here, inside the loop
	int shoulderTarPos = 0;
	int elbowTarPos = 0;
	while (true){
		if(vexRT[Btn7U]){ //ArmUp
			shoulderTarPos = 4000;
			elbowTarPos = 333; //measure pot for button values
		}
		else if(vexRT[Btn7D]) { //ArmDown
			shoulderTarPos = 2000;
			elbowTarPos = 100;
		}
		else if(vexRT[Btn7L]) { //ArmDrop
			shoulderTarPos = 4000;
			elbowTarPos = 0;
		}
		else if(vexRT[Btn7R]){ //ArmSet
			shoulderTarPos = 4000;
			elbowTarPos = 4000;
		}

		driveControl(1);

		if(false){//!vexRT[Btn6DXmtr2]) {
			armControl(elbowTarPos, shoulderTarPos); //this requires the potentiometers to be connected
			} else {
			manualArmControl();
		}
	}
}
